//----------------------------------------------------------------------------------------------
//	Filename:	PointCloudSolver.h
//	Author:		Sandro Spina
//	Date:		21/06/2013
//----------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------
#include <queue>

#include <nanoflann.hpp>
#include <iostream>
#include <fstream>

#include <boost\interprocess\file_mapping.hpp>
#include <boost\interprocess\mapped_region.hpp>
#include <boost\filesystem\operations.hpp>
#include <boost\filesystem\path.hpp>
#include <boost\filesystem.hpp>
#include <boost\lexical_cast.hpp>
#include <boost\format.hpp>
//#include <boost\tokenizer.hpp>
#include <boost\algorithm\string.hpp>

#include <System\EngineKernel.h>
#include <Scene\Scene.h>
#include <Shape\PlanarWorldShapeForge.h>

#include "PointCloudSolver.h"
#include "Export.h"

using namespace Illumina::Core;

//*********************************************
//*********************************************
//PointCloud Solver Starts Here 
//*********************************************
//*********************************************
PointCloudSolver::PointCloudSolver(Environment* p_environment)
{
	m_pEnvironment = p_environment;
	m_pEngineKernel = p_environment->GetEngineKernel();
	history_viewing_enabled = false;
	//Material Group "PCMaterial" has been set in the script. Get it from the material manager.
	m_pMaterial = (MaterialGroup*) m_pEngineKernel->GetMaterialManager()->RequestInstance("PCMaterial");
	//Material with groupid 0 is the default Matte material stored in the map		
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour0", "Id=0;Reflectivity={0.0,0.0,0.0};"), 0);		
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour1", "Id=1;Reflectivity={0.0,0.0,0.5};"), 1);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour2", "Id=2;Reflectivity={1.0,0.3,0.9};"), 2);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour3", "Id=3;Reflectivity={0.2,0.0,0.8};"), 3);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour4", "Id=4;Reflectivity={0.3,0.0,0.6};"), 4);	
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour5", "Id=5;Reflectivity={0.4,0.7,0.4};"), 5);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour6", "Id=6;Reflectivity={0.0,1.0,0.0};"), 6);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour7", "Id=7;Reflectivity={0.0,0.0,1.0};"), 7);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour8", "Id=8;Reflectivity={1.0,1.0,0.0};"), 8);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour9", "Id=9;Reflectivity={0.0,1.0,1.0};"), 9);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour10", "Id=10;Reflectivity={1.0,0.0,1.0};"), 10);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour11", "Id=11;Reflectivity={0.6,0.5,0.4};"), 11);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour12", "Id=12;Reflectivity={0.5,0.0,0.0};"), 12);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour13", "Id=13;Reflectivity={0.0,0.5,0.0};"), 13);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour14", "Id=14;Reflectivity={0.0,0.5,0.5};"), 14);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour15", "Id=15;Reflectivity={0.5,0.5,0.0};"), 15);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour16", "Id=16;Reflectivity={0.5,0.0,0.5};"), 16);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour17", "Id=17;Reflectivity={0.6,0.2,0.9};"), 17);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour18", "Id=18;Reflectivity={0.7,0.4,0.0};"), 18);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour19", "Id=19;Reflectivity={0.0,0.5,0.4};"), 19);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour20", "Id=20;Reflectivity={0.2,0.2,0.7};"), 20);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour21", "Id=21;Reflectivity={0.8,0.5,0.0};"), 21);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour22", "Id=22;Reflectivity={0.0,0.8,0.5};"), 22);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour23", "Id=23;Reflectivity={0.8,0.0,0.1};"), 23);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour24", "Id=24;Reflectivity={0.3,0.7,0.5};"), 24);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour25", "Id=25;Reflectivity={0.7,0.1,0.2};"), 25);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour26", "Id=26;Reflectivity={0.2,0.7,0.1};"), 26);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour27", "Id=27;Reflectivity={0.2,0.1,0.8};"), 27);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour28", "Id=28;Reflectivity={0.8,0.4,0.2};"), 28);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour29", "Id=29;Reflectivity={0.1,0.8,0.7};"), 29);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour30", "Id=30;Reflectivity={0.8,0.0,0.9};"), 30);
	m_pMaterial->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "Colour31", "Id=31;Reflectivity={0.3,0.2,0.9};"), 31);
	
	MaterialGroup* m_pMaterialGrid = (MaterialGroup*) m_pEngineKernel->GetMaterialManager()->RequestInstance("GridMaterial");
	m_pMaterialGrid->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "GridColour1", "Id=32;Reflectivity={0.1,0.1,1.0};"), 0);
	m_pMaterialGrid->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "GridColour2", "Id=33;Reflectivity={0.1,1.0,0.1};"), 1);
	m_pMaterialGrid->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "GridColour3", "Id=34;Reflectivity={0.9,0.1,0.2};"), 2);
	m_pMaterialGrid->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "GridColour4", "Id=35;Reflectivity={0.9,0.1,0.4};"), 3);
	
	MaterialGroup* m_pMaterialLit = (MaterialGroup*) m_pEngineKernel->GetMaterialManager()->RequestInstance("LitMaterial");
	m_pMaterialLit->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "LitColour1", "Id=LitColour1;Reflectivity={0.95,0.95,1.0};"), 0);
	m_pMaterialLit->Add(m_pEngineKernel->GetMaterialManager()->CreateInstance("Matte", "LitColour2", "Id=LitColour2;Reflectivity={0.95,0.9,0.99};"), 1);

	std::stringstream mess; mess << "[PointCloudSolver] " << IPointCloud::GetNumberOfMaterials()<< " colour materials created.";
	(*(m_pEnvironment->GetLogger()))["PCS"]->Write(mess.str());

	GeometricPrimitive * pointcloud_geometry = (GeometricPrimitive*)(m_pEnvironment->GetScene()->GetSpace()->PrimitiveList[0]);
	KDTreePointCloud * m_scenePointcloud = (KDTreePointCloud*)pointcloud_geometry->GetShape();
	m_pPreloadedScenes = new std::map<int, PointCloudSegmentation*>();
	
	m_trainedModelsDB = new ModelTraining(m_pEnvironment);
	//If training needs to carried out, do it now ... otherwise use pre-trained models.
	if (m_pEnvironment->IsTraining())
	{
		std::cout	<< "\n[STARTING TO TRAIN]*****************************************************************"
					<< "\n[STARTING TO TRAIN]*****************************************************************" << std::endl;
		
		m_trainedModelsDB->LoadAndTrainModels(m_pEnvironment, (KDTreePointCloud*) (pointcloud_geometry->GetShape()), m_pMaterial);
		m_trainedModelsDB->ToString();
	}	//end training
	else
	{
		//Load any previously trained models - TODO
		m_trainedModelsDB->LoadTrainedModels(m_pEnvironment, (KDTreePointCloud*) (pointcloud_geometry->GetShape()), m_pMaterial);
		m_trainedModelsDB->ToString();
	}

	//****
	//Check if we need to scan the scene (an OBJ model) to generate the points
	if (m_scenePointcloud->GetToScan())
		PlanarWorldShapeForge::CreateUsingScannerFromOBJFile(m_scenePointcloud->GetTrunk(), m_pEnvironment, &(m_scenePointcloud->m_pointList), m_scenePointcloud->GetMinDist(),
		m_scenePointcloud->GetMaxDist(), m_scenePointcloud->GetRadius(), 1, m_scenePointcloud->GetNumberOfScans());
	//Reset shape to the original kdtreepointcloud instead of the mesh
	pointcloud_geometry->SetShape(m_scenePointcloud);
	//****

	//**Produce the segments/structure graphs for the scene to solve.
	pointcloud_geometry->SetShape(m_scenePointcloud);
	m_sceneToSolve = new PointCloudSegmentation(p_environment, pointcloud_geometry, m_scenePointcloud->GetPCSId(), 0.5f);
	m_sceneToSolve->WriteToBPCSFile("Output/PCS/"+boost::lexical_cast<std::string, int>(m_scenePointcloud->GetPCSId())+"/");
	//WritePointsToASCIIFile(m_scenePointcloud, "Output/PointClouds/after_segmentation.asc");		

	//Scene Parsed and Structure Graphs have been created and written to file	
	//Now load the locations of all shapes (meshes) in the db. These will be used during search to identify subgraphs in the solution
}

int PointCloudSolver::ComputeSearchSpace()
{
	//PointCloudSceneSolution *sol = new PointCloudSceneSolution(m_sceneToSolve, m_trainedModelsDB);	//For now let us assume there is only one solution.
	//if (sol->ComputeSolution() > 1) 
	//{		
		
	//}
	m_sceneSolution = new PointCloudSceneSolution(m_sceneToSolve, m_trainedModelsDB);	//For now let us assume there is only one solution.
	m_sceneSolution->ComputeSolution();
	return 1;
}

//Export solution to ILM script
void exportScene()
{ 
   SceneBuilder sb;
   Transformation t, t2;
  
   PathDescriptor i;
  
   PerspectiveCameraDescriptor c;
   c.Position.Set(2.5, 2.5, 2.5);
   c.Target = 0;
  
   ObjDescriptor s;
   s.Filename = "Model\\chairs\\chair1.obj";
  
   MatteDescriptor m;
   m.Reflectivity = 1.f;
  
   PointLightDescriptor l;
   l.Position = 0.f;
   l.Power = 100.f;
  
   sb.SetCamera(&c);
   sb.SetIntegrator(&i);
  
   sb.AddLight(&l);
   sb.AddPrimitive(t, &s, &m, NULL);
   sb.AddPrimitive(t2, &s, NULL, NULL);
   sb.Build("E:\\Data\\Development\\IlluminaPRT\\Resource\\Output\\SceneScripts\\test.ilm");
   //sb.Build("Output\\SceneScripts\\test.ilm");	
}

//**********************************************
//***********Graph Drawing Functions************
//***************************************//
//***PointCloud File Output Function*****//
//***************************************//
void PointCloudSolver::WritePointsToASCIIFile(KDTreePointCloud *p_pointCloud, std::string _filepath)
{		
	std::ofstream myfile;
	_filepath = _filepath.substr(0, _filepath.length()-4);
	std::cout << "[PointCloudSolver] Writing Points to ASCII File " << _filepath << std::endl;
	myfile.open (_filepath);
	for (int i = 0; i<p_pointCloud->GetSize(); i++) {
		myfile << p_pointCloud->m_pointList[i].m_vertex.Position.X << " " << p_pointCloud->m_pointList[i].m_vertex.Position.Y << " " << p_pointCloud->m_pointList[i].m_vertex.Position.Z << "\n";
	}
	myfile.close();	
}


/**TEMPORARY TESTING OF EIGEN LIBRARY******/
	/*Eigen::Matrix3f covariance = Eigen::Matrix3f();				
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver;
	
	std::vector<Vector3> points;
	std::vector<Vector3> OBBpoints;
	std::vector<Vector3> points_2;
	Random r;
	for (int i=0; i<300; i++) points.push_back(Vector3(r.NextFloat()*10, r.NextFloat()*2, r.NextFloat()*4));	
	int numOfPoints = points.size();
	float c00, c11, c22, c01, c02, c12;
	c00 = c11 = c22 = c01 = c02 = c12 = 0.0f;						
	Vector3 median = Vector3(0);								
	for (int j=0; j<numOfPoints; j++)
	{					
		median.X += points[j].X;
		median.Y += points[j].Y;
		median.Z += points[j].Z;							
	}
	
	median /= numOfPoints;

	//Now calculate diagonal of covariance matrix
	for (int j=0; j<numOfPoints; j++) 
	{			
		c00 += (points[j].X - median.X) * (points[j].X - median.X);
		c11 += (points[j].Y - median.Y) * (points[j].Y - median.Y);
		c22 += (points[j].Z - median.Z) * (points[j].Z - median.Z);
		c01 += (points[j].X - median.X) * (points[j].Y - median.Y);
		c02 += (points[j].X - median.X) * (points[j].Z - median.Z);
		c12 += (points[j].Y - median.Y) * (points[j].Z - median.Z);		
	}				
	
	covariance << c00 / numOfPoints, c01 / numOfPoints, c02 / numOfPoints, 
					c01 / numOfPoints, c11 / numOfPoints, c12 / numOfPoints, 
					c02 / numOfPoints, c12 / numOfPoints, c22 / numOfPoints;
				
	eigensolver.compute(covariance);			
	std::cout << "[TEST Eigenvectors 0]\n " << eigensolver.eigenvectors().col(0) << " " << std::endl; 	
	std::cout << "[TEST Eigenvectors 1]\n " << eigensolver.eigenvectors().col(1) << " " << std::endl; 	
	std::cout << "[TEST Eigenvectors 2]\n " << eigensolver.eigenvectors().col(2) << " " << std::endl; 
	std::cout << "[TEST Eigenvalues]\n " << eigensolver.eigenvalues() << " " << std::endl; 

	Vector3 obb_0(eigensolver.eigenvectors().col(0)[0], eigensolver.eigenvectors().col(0)[1], eigensolver.eigenvectors().col(0)[2]);
	Vector3 obb_1(eigensolver.eigenvectors().col(1)[0], eigensolver.eigenvectors().col(1)[1], eigensolver.eigenvectors().col(1)[2]);
	Vector3 obb_2(eigensolver.eigenvectors().col(2)[0], eigensolver.eigenvectors().col(2)[1], eigensolver.eigenvectors().col(2)[2]);

	//Now move all the points in the coordinate space of the OBB
	for (int i=0; i<points.size(); i++) OBBpoints.push_back(Vector3(Vector3::Dot(points[i], obb_0), Vector3::Dot(points[i], obb_1), Vector3::Dot(points[i], obb_2)));

	float world_0_min = 1000000.0f; float world_0_max = -1000000.0f;
	float world_1_min = 1000000.0f; float world_1_max = -1000000.0f;
	float world_2_min = 1000000.0f; float world_2_max = -1000000.0f;

	for (int i=0; i<points.size(); i++) 
	{
		if (points[i].X > world_0_max) world_0_max = points[i].X;
		if (points[i].Y > world_1_max) world_1_max = points[i].Y;
		if (points[i].Z > world_2_max) world_2_max = points[i].Z;

		if (points[i].X < world_0_min) world_0_min = points[i].X;
		if (points[i].Y < world_1_min) world_1_min = points[i].Y;
		if (points[i].Z < world_2_min) world_2_min = points[i].Z;
	}

	float obb_0_d_min = 1000000.0f; float obb_0_d_max = -1000000.0f;
	float obb_1_d_min = 1000000.0f; float obb_1_d_max = -1000000.0f;
	float obb_2_d_min = 1000000.0f; float obb_2_d_max = -1000000.0f;

	for (int i=0; i<OBBpoints.size(); i++) 
	{
		if (OBBpoints[i].X > obb_0_d_max) obb_0_d_max = OBBpoints[i].X;
		if (OBBpoints[i].Y > obb_1_d_max) obb_1_d_max = OBBpoints[i].Y;
		if (OBBpoints[i].Z > obb_2_d_max) obb_2_d_max = OBBpoints[i].Z;

		if (OBBpoints[i].X < obb_0_d_min) obb_0_d_min = OBBpoints[i].X;
		if (OBBpoints[i].Y < obb_1_d_min) obb_1_d_min = OBBpoints[i].Y;
		if (OBBpoints[i].Z < obb_2_d_min) obb_2_d_min = OBBpoints[i].Z;
	}

	for (int i=0; i<OBBpoints.size(); i++) points_2.push_back((OBBpoints[i].X * obb_0) + (OBBpoints[i].Y * obb_1) + (OBBpoints[i].Z * obb_2));

	std::cout << world_0_max - world_0_min << "," << world_1_max - world_1_min << "," << world_2_max - world_2_min << " <-> " << obb_0_d_max - obb_0_d_min << "," << obb_1_d_max - obb_1_d_min << "," << obb_2_d_max - obb_2_d_min << std::endl;

	for (int i=0; i<OBBpoints.size(); i++) std::cout << points[i].ToString() << " -> " << OBBpoints[i].ToString() << " -> " << points_2[i].ToString() << std::endl;
	
	Plane p_0_min = Plane(obb_0, obb_0_d_min);
	Plane p_0_max = Plane(obb_0, obb_0_d_max);
	Plane p_1_min = Plane(obb_1, obb_1_d_min);
	Plane p_1_max = Plane(obb_1, obb_1_d_max);
	Plane p_2_min = Plane(obb_2, obb_2_d_min);
	Plane p_2_max = Plane(obb_2, obb_2_d_max);

	bool in = false;
	Vector3 test_point(4,1,2);
	if (p_0_min.GetSide(test_point) == Plane::Side_Positive)
		if (p_0_max.GetSide(test_point) == Plane::Side_Negative)
			if (p_1_min.GetSide(test_point) == Plane::Side_Positive)
				if (p_1_max.GetSide(test_point) == Plane::Side_Negative)
					if (p_2_min.GetSide(test_point) == Plane::Side_Positive)
						if (p_2_max.GetSide(test_point) == Plane::Side_Negative)
							in = true;

	std::cout << "Point " << test_point.ToString() << " is OBB? " << in << std::endl;
	*/
	/**************************************/
	/**************************************/
	/**************************************/
